function [F, cost, number_of_scalings, order] = AlMohyHigham2009(T)
%EXPM  Matrix exponential.
%   A. H. Al-Mohy and N. J. Higham, A new scaling and squaring algorithm
%      for the matrix exponential, SIAM J. Matrix Anal. Appl., 31(3),
%      (2009), pp. 970-989.
% This implementation follows exactly the paper.
% We adjusted theta value for r_13 (to 4.25) as indicated 
% in the paper and compute powers only when needed

    % Compute exponential
    % Get scaling and Pade parameters.
    [s, m, Tpowers, estimator_cost] = expm_params(T);
    number_of_scalings = s;
    order              = 2*m;

    % Rescale the powers of T appropriately.
    if s ~= 0
        T = T/(2.^s);
        Tpowers = cellfun(@rdivide, Tpowers, ...
            num2cell(2.^(s * (1:length(Tpowers)))), 'UniformOutput', false);
    end

    % Evaluate the Pade approximant.
    [F, extra_cost] = pade_approx(T, Tpowers, m);

    % total cost
    cost = estimator_cost + extra_cost;
    % Squaring phase.
    for k = 1:s
        F = F*F;
        cost = cost + 1;
    end
end

% Subfunctions
function [F, cost] = pade_approx(T, Tpowers, m)
%pade_approx Computes the Pade approximant to exp(T) of order [m/m].
c = get_pade_coefficients(m);
n = length(T);
I = eye(n,class(T));
cost = 0;
switch m
    case {3, 5, 7, 9}
        strt = length(Tpowers) + 2;
        for k = strt:2:m-1
            Tpowers{k} = Tpowers{k-2}*Tpowers{2};
            cost = cost + 1;
        end
        U = c(2)*I;
        V = c(1)*I; 
        for j = m:-2:3
            U = U + c(j+1)*Tpowers{j-1};
            V = V + c(j)*Tpowers{j-1};
        end
        U = T*U;
        cost = cost+1;
    case 13
        U = T * (Tpowers{6}*(c(14)*Tpowers{6} + c(12)*Tpowers{4} + ...
            c(10)*Tpowers{2}) + c(8)*Tpowers{6} + c(6)*Tpowers{4} + ...
            c(4)*Tpowers{2} + c(2)*I);
        cost = cost + 2;
        V = Tpowers{6}*(c(13)*Tpowers{6} + c(11)*Tpowers{4} + ...
            c(9)*Tpowers{2}) + c(7)*Tpowers{6} + c(5)*Tpowers{4} + ...
            c(3)*Tpowers{2} + c(1)*I;
        cost = cost + 1;
end
warns = warning('off','MATLAB:nearlySingularMatrix');
try
   F = (V-U)\(2*U) + I;  %F = (-U+V)\(U+V);
   cost = cost + 4/3;
   warning(warns); 
catch e
   warning(warns); 
   rethrow(e);
end
end

function c = get_pade_coefficients(m)
%get_pade_coefficients Coefficients of numerator P of Pade approximant
%    C = get_pade_coefficients returns coefficients of numerator
%    of [m/m] Pade approximant, where m = 3,5,7,9,13.
switch m
    case 3
        c = [120, 60, 12, 1];
    case 5
        c = [30240, 15120, 3360, 420, 30, 1];
    case 7
        c = [17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1];
    case 9
        c = [17643225600, 8821612800, 2075673600, 302702400, 30270240, ...
            2162160, 110880, 3960, 90, 1];
    case 13
        c = [64764752532480000, 32382376266240000, 7771770303897600, ...
            1187353796428800,  129060195264000,   10559470521600, ...
            670442572800,      33522128640,       1323241920,...
            40840800,          960960,            16380,  182,  1];
end
end
        
function t = ell(T, coeff, m_val)
    %ell Function needed to compute optimal parameters.
    scaledT = coeff.^(1/(2*m_val+1)) .* abs(T);
    alpha = normAm(scaledT,2*m_val+1)/norm(T,1);
    t = max(ceil(log2(2*alpha/eps(class(alpha)))/(2*m_val)),0);
end

function [s, m, Tpowers, cost] = expm_params(T)
%expm_params Obtain scaling parameter and order of the Pade approximant.
% Coefficients of backwards error function.
    coeff = [1/100800, 1/10059033600, 1/4487938430976000,...
         1/5914384781877411840000, 1/113250775606021113483283660800000000];

    s = 0;
    % m_val is one of [3 5 7 9 13];
    % theta_m for m=1:13.
    theta = [%3.650024139523051e-008
        %5.317232856892575e-004
        1.495585217958292e-002  % m_vals = 3
        %8.536352760102745e-002
        2.539398330063230e-001  % m_vals = 5
        %5.414660951208968e-001
        9.504178996162932e-001  % m_vals = 7
        %1.473163964234804e+000
        2.097847961257068e+000  % m_vals = 9
        %2.811644121620263e+000
        %3.602330066265032e+000
        %4.458935413036850e+000
        4.25]; % SUGGESTED VALUE FROM PAPER (p.14) instead of 5.37
        %5.371920351148152e+000];% m_vals = 13

    Tpowers{2} = T*T;
    cost       = 1;
    d4 = normAm(Tpowers{2},2)^(1/4);
    d6 = normAm(Tpowers{2},3)^(1/6);
    eta1 = max(d4, d6);
    if (eta1 <= theta(1) && ell(T, coeff(1), 3) == 0)
        m = 3;
        return;
    end
    Tpowers{4} = Tpowers{2}*Tpowers{2};
    cost       = cost + 1;
    d4 = normAm(Tpowers{4},1)^(1/4);    % Update norm estimator
    eta1 = max(d4, d6);
    if (eta1 <= theta(2) && ell(T, coeff(2), 5) == 0)
        m = 5;
        return;
    end
    Tpowers{6} = Tpowers{2}*Tpowers{4};
    cost       = cost + 1;
    d6 = normAm(Tpowers{6}, 1)^(1/6);   % Update norm estimator
    d8 = normAm(Tpowers{4}, 2)^(1/8);
    eta3 = max(d6, d8);
    if (eta3 <= theta(3) && ell(T, coeff(3), 7) == 0)
        m = 7;
        return;
    end
    if (eta3 <= theta(4) && ell(T, coeff(4), 9) == 0)
        m = 9;
        return;
    end
    d10 = normAm(Tpowers{2}, 5)^(1/10);

    eta4 = max(d8, d10);
    eta5 = min(eta3, eta4);
    s = max(ceil(log2(eta5/theta(5))), 0);
    s = s + ell(T/2^s, coeff(5), 13);
    if isinf(s)
        % Overflow in ell subfunction. Revert to old estimate.
        [t, s] = log2(norm(T,1)/theta(end));
        s = s - (t == 0.5); % adjust s if normA/theta(end) is a power of 2.
    end
    m = 13;
end

function [c,mv] = normAm(A,m)
%NORMAM   Estimate of 1-norm of power of matrix.
%   NORMAM(A,M) estimates norm(A^m,1). If A has nonnegative elements 
%   the estimate is exact.
%   [C, MV] = NORMAM(A,M) returns the estimate C and the number MV of
%   matrix-vector products computed involving A or A^*.
%   Reference: 
%   A. H. Al-Mohy and N. J. Higham, A New Scaling and Squaring Algorithm 
%      for the Matrix Exponential, SIAM J. Matrix Anal. Appl. 31(3):
%      970-989, 2009.%
%   Awad H. Al-Mohy and Nicholas J. Higham
%   Copyright 2014-2015 The MathWorks, Inc.
if isreal(A) && all(A(:) >= 0)
  % For positive matrices only.
  e = ones(n,1,class(A));
  for j=1:m
      e = A'*e;
  end
  c = norm(e,inf);
  mv = m;
else
  [c,~,~,it] = normest1(@afun_power);
  mv = it(2)*2*m; % Since t = 2.
end
% End of normAm

function Z = afun_power(flag,X)
        %afun_power  Function to evaluate matrix products needed by normest1.
        if isequal(flag,'dim')
            Z = n;
        elseif isequal(flag,'real')
            Z = isreal(A);
        else
            if isequal(flag,'notransp')
                for i = 1:m
                    X = A*X;
                end
            elseif isequal(flag,'transp')
                for i = 1:m
                    X = A'*X;
                end
            end
            Z = X;
        end
    end
end
